/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    sim_interface.cpp
  * @author  Mentos Seetoo
  * @brief   Interfaces of simulation. Used in robot luancher.																	 
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure your code does not have any 
  * bugs, update the version Number, write dowm your name and the date.What most
  * important is to make sure the users will have clear an understanding through
  * your new brief.
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "Simulation/sim_interface.h"
#include <iostream>

/* Private define ------------------------------------------------------------*/

/* Functions -----------------------------------------------------------------*/
void SimInterface::init()
{
    std::cout << "[CoppeliaSim Client] Successfully created service thread ! \n";
#ifdef LOCAL_SIMULATION
    hClient = &CoppeliaSim_Client::getInstance();
    std::cout << "[CoppeliaSim Client] Initializing service, setting parameters... \n";
    
    hClient->Set_startParam("127.0.0.1", 5000, 5, true);
    std::cout << "[CoppeliaSim Client] Connecting to server.. \n";
    /*Make sure to connect*/
    while (!hClient->Start("127.0.0.1", 5000, 5, true)){};
    std::cout << "[CoppeliaSim Client] Successfully connected to server, configuring...\n";
    _configSimulation();
    std::cout << "[CoppeliaSim Client] Configure done, simulation interface is ready ! \n";
#else
    std::cout << "[CoppeliaSim Client] Service is not enabled at compile time! \n";
#endif
}

void SimInterface::task()
{
    static int cnt = 0;
#ifdef LOCAL_SIMULATION
    _sendToSimulation();
    if (hClient->Is_Connected())
    {
        hClient->ComWithServer();

        /*Synchronous with sim-interfaces*/
        printf("SIm Thread finished %d \n", cnt++);
    }
    _readFromSimulation();
#else
    {;}
#endif
}

/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/